/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it
under the terms of the GNU Lesser General Public License as published
by the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
License for more details.

You should have received a copy of the GNU Lesser General Public
License along with GIMnet. (See COPYING.LESSER) If not, see
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: JointGroupCtrlClientExample.cpp,v 1.4 2009-08-27 14:12:35 morsko Exp $
 *
 * \file
 * \brief This is an example for using the JointGroupCtrl interface.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *edited by Tilman Pfeiffer to fit the FSR 2011 Group 5 needs
 */
#include "interface_header.hpp"
#include "JointGroupCtrlClient.hpp"
#include <curses.h>
#include <math.h>
//*****************************************************************************
#define GIMI_CLIENT_API 20000
#define TORAD 0.017453292
#define TODEG 57.29577951
#include "gimi.h"
//*****************************************************************************
// Objects
static bool run = true;
static gimi::GIMI g;
static MaCI::JointGroupCtrl::CJointGroupCtrlClient jgcc(&g, 0);


//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
static int verbose = 1;
//*****************************************************************************
//*****************************************************************************

static void DoJointGroupCtrl()
{
  // Initialize Curses (After this point, don't use printf family calls!)
  initscr();
  cbreak();
  //noecho();
  nonl();
  intrflush(stdscr, FALSE);
  keypad(stdscr, TRUE);
  scrollok(stdscr, TRUE);
  //nodelay(stdscr, TRUE);
  //halfdelay(10);

  // After this point, we don't want dPrint to print to Screen, so we disable it.
  debugSetPrintLevel(0);

  unsigned int speedSeq = 0;
  unsigned int positionSeq = 0;
  //unsigned int forceSeq = 0;
  clear();
  printw("Welcome to IO test program.\n\n");
  using namespace MaCI::JointGroupCtrl;
  do {
    int value;
    printw("\nSelect JointGroupCtrl Query:\n");
    printw(
   "KEY_UP = x\n"
	   "KEY_DOWN = -x\n"
	   "KEY_LEFT = y\n"
	   "KEY_RIGHT= -y\n"
	   " 4 = wrist left\n"
	   " 6 = wrist right\n"
	   " 8 = vertical up\n"
	   " / = vertical all up\n"
       " 2 = vertical down\n"
       " 5 = vertical all down\n"
	   " 7 = gripper close\n"
       " 9 = gripper open\n"
       " 1 = direct angle -S\n"
       " 3 = direct angle S\n"
       " 0 = direct angle -E\n"
       " , = direct angle E\n"
        "s = Set joint Speed\n"
           "S = Set speeds of many joints\n"
	   "a = Get joint Speed\n"
           "A = Get speeds of multiple Joints\n"
	   "p = Set joint Position\n"
           "P = Set position of many joints\n"
	   "o = Get joint Position\n"
           "O = Get positions of multiple joints\n"
	   "f = Set joint Force\n"
           "F = Set forces of many joints\n"
	   "d = Get joint Force\n"
           "D = Get forces of multiple joints\n"
	   "I = Get joint Info\n"
	   "n = Get number of joints\n"
	   "i = Get Jointgroup information\n"
           "j = Get jointgroup status\n"
	   "q = QUIT\n");
    printw("?\n");

    value = getch();
    clear(); // After input, clear the screen. Not after processing!
    switch(value){

 //get current position and add an increment to it to move step by step
    case KEY_UP:{//x
        float pos;
        jgcc.GetPosition(0,pos,1000);
        jgcc.SetPosition(20+pos,0);
        break;
    }
    case KEY_DOWN:{//-x
	        float pos;
            jgcc.GetPosition(0,pos,1000);
            jgcc.SetPosition(-20+pos,0);
        break;
    }
    case KEY_LEFT:{//-y
	        float pos;
            jgcc.GetPosition(1,pos,1000);
            jgcc.SetPosition(20+pos,1);
        break;
    }
    case KEY_RIGHT:{//y
	        float pos;
            jgcc.GetPosition(1,pos,1000);
            jgcc.SetPosition(-20+pos,1);;
        break;
    }
    case '1':{//shoulder left
	        float pos;
            jgcc.GetPosition(4,pos,1000);
            jgcc.SetPosition(10+pos*TODEG,4);
        break;
    }
    case '3':{//shoulder right
	        float pos;
            jgcc.GetPosition(4,pos,1000);
            jgcc.SetPosition(-10+pos*TODEG,4);
        break;
    }
    case '0':{//elbow left
	        float pos;
            jgcc.GetPosition(5,pos,1000);
            jgcc.SetPosition(10+pos*TODEG,5);
        break;
    }
    case ',':{//elbow right
	        float pos;
            jgcc.GetPosition(5,pos,1000);
            jgcc.SetPosition(-10+pos*TODEG,5);
        break;
    }
    case '8':{//linerActuatorUP
	        float pos;
            jgcc.GetPosition(6,pos,1000);
            jgcc.SetPosition(-25+pos,6);
        break;
    }

    case '2':{//linearActuatorDown
	        float pos;
            jgcc.GetPosition(6,pos,1000);
            jgcc.SetPosition(25+pos,6);
        break;
    }
    case '4':{//wrist left
            float pos;
            jgcc.GetPosition(7,pos,1000);
            jgcc.SetPosition(25+pos,7);
        break;
    }
    case '6':{//wrist right
	        float pos;
            jgcc.GetPosition(7,pos,1000);
            jgcc.SetPosition(-25+pos,7);
        break;
    }
    case '7':{//gripper open
            float pos;
            jgcc.GetPosition(8,pos,1000);
            jgcc.SetPosition(-10+pos,8);
        break;
    }
    case '9':{//gripper close
	        float pos;
            jgcc.GetPosition(8,pos,1000);
            jgcc.SetPosition(10+pos,8);
        break;
    }

    //----------------------end of step by step input.

    case '5':{//linear actuator to maximum extension
	jgcc.SetPosition(255,6);
    break;
    }
    case '/':{//linear actuator to minimum extension
	jgcc.SetPosition(0,6);
    break;
    }


    case 's':{
      int r;
      float speed;
      int jointNumber;
      printw("Enter speed and joint Number: ");
      r = scanw("%f %d",&speed, &jointNumber);
      if (r >= 2) {
	jgcc.SetSpeed(speed,jointNumber);
      }
      break;
    }
    case 'S':{
      float speed;
      int jointNumber,r;
      printw("Enter speed and joint Number (Leave empty when finished): ");
      r = scanw("%f %d",&speed, &jointNumber);

      TJointSpeedArray array;
      while(r >=2){

	array.push_back(TJointSpeed(speed,jointNumber));
        printw("Enter speed and joint Number (Leave empty when finished): ");
        r = scanw("%f %d",&speed, &jointNumber);
      }
      jgcc.SetGroupSpeed(array);
      break;
    }
    case 'p':{
      float position;
      int jointNumber,r;
      printw("Enter position and joint Number: ");
      r = scanw("%f %d",&position, &jointNumber);
      if (r >= 2) {
	jgcc.SetPosition(position,jointNumber);
      }

      break;
    }
    case 'P':{
      float position;
      int jointNumber,r;
      printw("Enter position and joint Number (Leave empty when finished): ");
      r = scanw("%f %d",&position, &jointNumber);

      TJointPositionArray array;
      while(r >=2){

        array.push_back(TJointPosition(position,jointNumber));
        printw("Enter position and joint Number (Leave empty when finished): ");
        r = scanw("%f %d",&position, &jointNumber);
      }
      jgcc.SetGroupPosition(array);
      break;
    }
    case 'f':{
      float force;
      int jointNumber,r;
      printw("Enter force and joint Number: ");
      r = scanw("%f %d",&force, &jointNumber);
      if (r >= 2) {
	jgcc.SetForce(force,jointNumber);
      }

      break;
    }
    case 'F':{
      float force;
      int jointNumber,r;
      printw("Enter force and joint Number (Leave empty when finished): ");
      r = scanw("%f %d",&force, &jointNumber);

      TJointForceArray array;
      while(r >=2){

	array.push_back(TJointForce(force,jointNumber));
        printw("Enter force and joint Number (Leave empty when finished): ");
        r = scanw("%f %d",&force, &jointNumber);
      }
      jgcc.SetGroupForce(array);
      break;

    }
    case 'I':{
      int jointNumber, r;
      bool ret;
      MaCI::JointGroupCtrl::TJointInfo info("fuubar", 0, MaCI::JointGroupCtrl::KJointControlAngular);
      printw("Enter joint Number: ");
      r = scanw("%d", &jointNumber);
      if (r >= 1) {
        ret = jgcc.GetJointInformation(info,jointNumber,1000);
      }
      if (!ret){
		printw("FAILED!\n");
		break;
      }
	  printw("%d. name: %s, number %d",jointNumber,
		 info.jointName.c_str(),
		 info.jointNumber);
	  if(info.jointControlType == MaCI::JointGroupCtrl::KJointControlAngular){
	    printw(", takes control commands with angular units,");
	  }else if(info.jointControlType == MaCI::JointGroupCtrl::KJointControlLinear){
	    printw(", takes control commands with length units,");
	  }

	  if(info.canControlSpeed){
	    printw(" Can be controlled by speed commands, maxSpeed %f",info.maxSpeed);
	  }
	  if(info.canControlPosition){
	    printw(" Can be controlled by position commands, minPosition %f, maxPosition %f",
		   info.minPosition,
		   info.maxPosition);
	  }
          if(info.canControlForce){
            printw(" Can be controlled by force commands, minForce %f, maxForce %f",
                   info.minForce,
                   info.maxForce);
          }
	  printw("\n");

      break;
    }
    case 'i':{
      MaCI::JointGroupCtrl::TJointGroupInformation info;
      printw("Getting information...");
      if(jgcc.GetJointGroupInformation(info,1000)){
	printw("SUCCESS!\n");
	int numberOfJoints = info.size();
	for(int i = 0; i< numberOfJoints; i++){
	  printw("%d. name: %s, number %d",i,
		 info[i].jointName.c_str(),
		 info[i].jointNumber);
	  if(info[i].jointControlType == MaCI::JointGroupCtrl::KJointControlAngular){
	    printw(", It is an angular joint");
	  }else if(info[i].jointControlType == MaCI::JointGroupCtrl::KJointControlLinear){
	    printw(", It is a linear joitn");
	  }

	  if(info[i].canControlSpeed){
	    printw(", Can be controlled by speed commands, maxSpeed %f",info[i].maxSpeed);
	  }
	  if(info[i].canControlPosition){
	    printw(", Can be controlled by position commands, minPosition %f, maxPosition %f",
		   info[i].minPosition,
		   info[i].maxPosition);
	  }
          if(info[i].canControlForce){
            printw(", Can be controlled by force commands, minForce %f, maxForce %f",
                   info[i].minForce,
                   info[i].maxForce);
          }
	  printw("\n");

	}

      }else{
	printw("FAILED!\n");
      }

      break;
    }
    case 'a':{
      int jointNumber,r;
      float speed;
      unsigned int timeout = 1000;
      printw("Enter joint Number: ");
      r = scanw("%d", &jointNumber);
      if (r >= 1) {
	printw("Getting speed (timeout %u ms)...",timeout);
	if(jgcc.GetSpeed(jointNumber,speed,timeout)){
	  printw("SUCCESS!\n");
	  printw("Joint n.%d speed is %f",
		 jointNumber,
		 speed);
	}else{
	printw("FAILED!\n");
	}
      }
      break;
    }
    case 'A':{

      unsigned int timeout = 1000;
      printw("Getting group speeds (timeout %u ms)... ",timeout);
      TJointSpeedArray speedArray;

      if(jgcc.GetGroupSpeeds(speedArray,&speedSeq,timeout)){
        printw("SUCCESS!\n");
        for(EACH_IN_i(speedArray)){
          printw("Joint n.%d speed is %f\n",
		 i->jointNumber,
		 i->speed);
        }
      }else{
	printw("FAILED!\n");
      }
      break;


    }
    case 'o':{
      int jointNumber,r;
      float position;
      unsigned int timeout = 1000;
      printw("Enter joint Number: ");
      r = scanw("%d", &jointNumber);
      if (r >= 1) {
	printw("Getting position (timeout %u ms)...",timeout);
	if(jgcc.GetPosition(jointNumber,position,timeout)){
	  printw("SUCCESS!\n");
	  printw("Joint n.%d position is %f",
		 jointNumber,
		 position);
	}else{
	printw("FAILED!\n");
	}
      }
      break;
    }
    case 'O':{

      unsigned int timeout = 1000;
      printw("Getting group positions (timeout %u ms)... ",timeout);
      TJointPositionArray positionArray;
      if(jgcc.GetGroupPositions(positionArray,&positionSeq,timeout)){
        printw("SUCCESS!\n");
        for(EACH_IN_i(positionArray)){
          printw("Joint n.%d position is %f\n",
		 i->jointNumber,
		 i->position);
        }
      }else{
	printw("FAILED!\n");
      }
      break;

    }
  /*  case 'd':{
      int jointNumber,r;
      float force;
      unsigned int timeout = 1000;
      printw("Enter joint Number: ");
      r = scanw("%d", &jointNumber);
      if (r >= 1) {
	printw("Getting force (timeout %u ms)...",timeout);
	if(jgcc.GetForce(jointNumber,force,timeout)){
	  printw("SUCCESS!\n");
	  printw("Joint n.%d force is %f",
		 jointNumber,
		 force);
	}else{
          printw("FAILED!\n");
	}
      }
      break;
    }
    case 'D':{

      unsigned int timeout = 1000;
      printw("Getting group forces (timeout %u ms)... ",timeout);
      TJointForceArray forceArray;

      if(jgcc.GetGroupForces(forceArray,&forceSeq,timeout)){
        printw("SUCCESS!\n");
        for(EACH_IN_i(forceArray)){
          printw("Joint n.%d force is %f\n",
		 i->jointNumber,
		 i->force);
        }
      }else{
	printw("FAILED!\n");
      }
      break;


    }*/
    case 'n':{
      int j;
      printw("Getting joint Number... ");
	  j = jgcc.GetJointNumber();
	  printw("%d joints found \n", j);
      break;
    }

    case 'j':{
      MaCI::JointGroupCtrl::EJointGroupStatus status;
      printw("Getting group status...");
      if(jgcc.GetJointGroupStatus(status,1000)){
        printw("SUCCESS!\n");
	  printw("Joint group status is %d (open = %d, closed = %d, unimplemented = %d",
		 status,
                 MaCI::JointGroupCtrl::KJointGroupStatusOpen,
                 MaCI::JointGroupCtrl::KJointGroupStatusClosed,
                 MaCI::JointGroupCtrl::KJointGroupStatusUnimplemented);

      }else{
	printw("FAILED!\n");
      }

      break;

    }
    case 'q':{

      run = false;
      printw("\nBYE BYE!.\n\n");

      break;
    }
    default:{
      clear();
      printw("\nUnknown command.\n\n");
      continue;
      break;
    }
    }

 } while(run);

  // Close curses.
  endwin();

}


//*****************************************************************************
void showUsage()
{
  fprintf(stderr,"\n");
  fprintf(stderr,"Usage:\tJointGroupCtrlClientExample [OPTIONS]\n");
  fprintf(stderr,"  [-u addr]\t\t\tUse 'addr' as GIMnet AccessPoints address\n");
  fprintf(stderr,"  [-p port]\t\t\tUse 'port' as GIMnet AccessPoints port\n");
  fprintf(stderr,"  [-n name]\t\t\tUse 'name' as this nodes GIMnet name\n");
  fprintf(stderr,"  [-s name]\t\t\tUse 'name' as SpeedCtrl target\n");
  fprintf(stderr,"  \t\t\t\tOR\n");
  fprintf(stderr,"  [-S name]\t\t\tSpecify the data source by MaCISL\n\n");
  fprintf(stderr,"  [-v]\t\t\t\tIncrease verbosity by 1\n");
  fprintf(stderr,"  [-q]\t\t\t\tQuiet mode\n");
  fprintf(stderr, "\n\n");
}
//*****************************************************************************
MarsManipulator::MarsManipulator(){

//int main(int argc, char *argv[]) {
  // GIMnet parameters
  std::string gimnetAP = "localhost";
  int gimnetAPPort = 40002;
  std::string gimnetName = "mars";
  std::string gimnetSourceName = "Control";
  std::string datasourceMaCISL;
  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(8);
  debugSetLogFilename("JointGroupCtrl_Client_example.log");
  debugSetLogLevel(6);
  // Greet
  fprintf(stderr,
	  "\nJointGroupCtrlClientExample - MaCI JointGroupCtrl Interface Example - v0.0\n");

  // Parse parameters
  bool parse = true;
  /*
  while(parse) {
    int c;
    c = getopt(argc, argv, "s:S:n:u:p:hqv");
    switch(c) {

      /////////////////////////////////
      ////// Application specific /////
      /////////////////////////////////

      /////////////////////////////////
      ///// GIMnet parameters /////////
      /////////////////////////////////
    case 's':
      gimnetSourceName = optarg;
      dPrint(3,"Using GIMnet source name '%s'", optarg);
      break;

    case 'S':
      datasourceMaCISL = optarg;

      dPrint(3,"Using DataSource MaCISL '%s'", optarg);
      break;

    case 'n':
      gimnetName = optarg;
      dPrint(3,"Using GIMnet Name '%s'", optarg);
      break;

    case 'u':
      gimnetAP = optarg;
      dPrint(3,"Using GIMnetAP address '%s'", optarg);
      break;

    case 'p':
      gimnetAPPort = atoi(optarg);
      dPrint(3,"Using GIMnetAP port '%d'", gimnetAPPort);
      break;

      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////
    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0)
	debugSetGlobalDebugLvl(++verbose);
      break;

    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(0);
      break;

    case 'h':
    case '?':
      showUsage();
      exit(1);
      break;

    case -1:
      parse = false;
      break;
    }
  }

*/
  MaCI::EMaCIError e;

  int r;
  if ( (r = g.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(),
	   gimnetAPPort,
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());

  } else {
    dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(),
	   gimnetAPPort,
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());



    // Assign default target.
    if (gimnetSourceName.size()) {
      jgcc.SetDefaultTarget(gimnetSourceName);

    } else {
      // GIMI is Open(), so we can execute this.
      MaCI::MaCICtrl::SMaCISL sl(datasourceMaCISL);
      if (sl.IsValidFQMaCISL()) {
        jgcc.SetDefaultTarget(sl, 10000);
      }

    }
    // Attempt to Open Client
    if ( (e = jgcc.Open()) != MaCI::KMaCIOK) {
      dPrint(1,"Failed to Open JointGroupCtrlClient instance! (%s)",
	     GetErrorStr(e).c_str());

    } else {

      //DoJointGroupCtrl();
      //jgcc.Close();
    }
  }


}

MarsManipulator::~MarsManipulator(){
    jgcc.Close();
}

	// Picking up objects
MarsManipulator::ECommandResult MarsManipulator::pickObject(double atx,double aty){return UNKNOWN; }
MarsManipulator::ECommandResult MarsManipulator::extendArm(){
    jgcc.SetPosition(300,0);
    jgcc.SetPosition(0,1);
    return UNKNOWN;
}
MarsManipulator::ECommandResult MarsManipulator::retractArm(){return UNKNOWN;}

// Storing and delivering objects
MarsManipulator::ECommandResult MarsManipulator::deliverFromManipulator(){return UNKNOWN;}
MarsManipulator::ECommandResult MarsManipulator::deliverFromPocket(int pocket){return UNKNOWN;}
MarsManipulator::ECommandResult MarsManipulator::storeObjectToPocket(int pocket){return UNKNOWN;}

//manipulator control
MarsManipulator::ECommandResult MarsManipulator::closeManipulator(){return UNKNOWN;}
MarsManipulator::ECommandResult MarsManipulator::openManipulator(){
	DoJointGroupCtrl();
	return UNKNOWN;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
